Skip to content

feat(autoware): use nalgebra for common types - #775

Open
romancardenas wants to merge 4 commits into
tier4:mainfrom
romancardenas:dev-748-crop-box
Open

romancardenas wants to merge 4 commits into
tier4:mainfrom
romancardenas:dev-748-crop-box

Conversation

@romancardenas

Copy link
Copy Markdown
Contributor

Description

Instead of defining custom types for Vector3, Quaternion, etc., this PR uses nalgebra native types.

The main benefits is that we can use a lot of methods and functionalities provided by this package.

IMPORTANT: packages do not depend directly on nalgebra. Instead, common_types serves as a common framework where we re-export the functionalities we consider necessary.

I adapted the current autoware packages to follow this approach. The most important changes can be found in imu_corrector.

I think this change will allow us to better scale as we include functionalities to autoware. I am about to start addressing #748 and detected that this restructuring would be beneficial.

How was this PR tested?

  • scripts/ci.sh passes on my machine.
  • Autoware tests pass on my machine.

@romancardenas

Copy link
Copy Markdown
Contributor Author

@nokosaaan I pushed a new commit addressing your review.

I think that, eventually, this port can benefit from adapting data types to the Rust ecosystem, so probably using nalgebra's native data types could be beneficial. That is why I marked the new Transform::try_from_ros_xyzw as deprecated with proper documentation. It is OK to use it during porting efforts, but in the future we could adapt it to the nalgebra API.

Let me know what you think!

@nokosaaan

Copy link
Copy Markdown
Contributor

@romancardenas
Thanks for the update — the rotate() fix in gyro_odometer and the tf2-equivalent checks in try_new look good to me.

I agree with the direction: common_types should expose nalgebra types, and ROS-ordered data should only appear at I/O boundaries (CSV/UDP parsers, the ROS bridge). Even with sensing→localization fully in Rust, that is the only place where (x, y, z, w) quaternions come in or go out.

Regarding try_from_ros_xyzw: let's keep it as it is (deprecated) for now. We are planning a refactoring of the static-TF handling, and I'll clean it up there. The plan is roughly:

  • Support both static-TF sources: the calibration YAMLs (xyz + rpy) and rosbag /tf_static (xyzw), each through its own loader in the I/O layer, both feeding the same Transform.
  • Add Transform::from_xyz_rpy (via UnitQuaternion::from_euler_angles, same convention as tf2 setRPY) and composition (base_link → sensor_kit_base_link → imu_link) to common_types.
  • Move the xyzw handling into the /tf_static loader as a private helper on top of your try_new validation, and remove try_from_ros_xyzw from common_types at that point.
  • Possibly back Transform with nalgebra::Isometry3, which would give us transform_point / transform_vector and composition for free.
  • Rename MockTransformListener (e.g. StaticTransformListener) with an allocation-free (&'static str, &'static str) key.

For this PR, I'd only ask for two small things, if you don't mind:

  1. A short module-level doc in common_types stating the convention: internal code uses nalgebra (Quaternion::new(w, i, j, k)); ROS (x, y, z, w) conversions happen only in I/O layers.
  2. One order-pinning test using Quaternion::new(w, ...) directly, e.g. a 90° yaw maps (1, 0, 0) to (0, 1, 0), and roll = π maps (ωx, ωy, ωz) to (ωx, −ωy, −ωz).

Other follow-ups I'll handle separately: the gyro_odometer frame_id / covariance fix, replacing ekf_localizer's own Quaternion struct with nalgebra, revisiting the positional TrajectoryPoint::new(…, qw, qi, qj, qk, …), and validating CSV headers by column name.

Thanks again for the quick iteration!

@romancardenas

Copy link
Copy Markdown
Contributor Author

@nokosaaan I addressed your requested changes!

@nokosaaan

Copy link
Copy Markdown
Contributor

@romancardenas
I don't have permission to resolve comments here, so could you click the resolve button yourself?

This branch has not been deployed

No deployments
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants